import sys
import time
import numpy as np
import cv2
import socket
from getL import getL

import math
import time





fgbg =cv2.createBackgroundSubtractorMOG2()
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(5, 5))

startTime = time.time()

def detect_laser(frame):
    global startTime
    fgmask =fgbg.apply(frame)
    mask = cv2.morphologyEx(fgmask, cv2.MORPH_OPEN, kernel)
    cv2.imshow("mask",mask)
    cv2.waitKey(1)

    _,contours,_ = cv2.findContours(fgmask.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    
    cnt = 0
    sum_x = 0
    sum_y = 0
    for c in contours:
        if 10 < cv2.contourArea(c) < 40000:
            x, y, w, h = cv2.boundingRect(c)
            cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 0, 255))
            sum_x += x + w / 2
            sum_y += y + h / 2
            cnt += 1
    cv2.imshow("result", frame)
    cv2.waitKey(1)
    if(cnt == 0):
        return 0
    else:
        return [sum_y / cnt, sum_x / cnt]


if __name__ == "__main__":
    cap = cv2.VideoCapture(0)
    cap.set(3,640)
    cap.set(4,480)
    
    cnt = 0
    resultList = []
    while cnt <= 200:
        cnt += 1
        flag, frame  = cap.read()
        timestamp = time.time()

        result = detect_laser(frame)
        if(result != 0):
            result.append(timestamp)
            print(result)
            resultList.append(result)
    
    resultList = np.array(resultList)
    print(len(resultList))
    L = getL(resultList)
    print("L: " + str(L))
    print("time consume: " + str(time.time() - startTime))




    

